﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using GeneralDefine;
using System.Windows.Forms;
using cszmcaux;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Button;
using DevExpress.Data.Linq.Helpers;
using DevExpress.Internal.WinApi.Windows.UI.Notifications;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.StartPanel;
using System.Runtime.Remoting.Channels;
using static DevExpress.Utils.Diagnostics.GUIResources;
using DevExpress.Utils;




namespace ZMotion
{
    //internal class MotorTaskGroup
    //{
       
    //    internal int[] iBeforePos;
    //    internal MotorTaskGroup()
    //    {

    //    }

    //}
    public class ZMotionCard : HardWareBase,IMotion,IInput,IOutput
    {

        public IntPtr g_handle;

        const Int32 YES = 1;
        const Int32 NO = 0;
        const Int32 ON = 1;
        const Int32 OFF = 0;
        static bool bInitCard = false;
        //static int totalCardNum = 0;
        public int iCardNO;
        public int iTotalAxisNum=32;
        public int StartAxisID=0;
        public int v_channel;
        public const int AxisMaxNum = 32;
        public ZMotionCardSetting cardSetting;
        public string strCardInfor;
        public string[] strCurrentStatus = new string[AxisMaxNum];
        public float[] iCommandPos = new float[AxisMaxNum];
        public double[] doubleCommandPos = new double[AxisMaxNum];
        public double[] doubleCurEncPos = new double[AxisMaxNum];
        public float[] iCurrentPos=new float[AxisMaxNum];
        private MotionSpeed[] currentSpd = new MotionSpeed[AxisMaxNum];
        private MotionSpeed[] settingSpd = new MotionSpeed[AxisMaxNum];
        private MotionSpeed[] cureSpd = new MotionSpeed[8];
        private bool[] bHomeSenser = new bool[AxisMaxNum];
        private bool[] bEstop = new bool[AxisMaxNum];
        private bool[] bServoOn = new bool[AxisMaxNum];
        private bool[] bCWLSenser = new bool[AxisMaxNum];
        private bool[] bCCWLSenser = new bool[AxisMaxNum];
        private bool[] bAlarmSenser = new bool[AxisMaxNum];
        private bool[] bMoveDone = new bool[AxisMaxNum];
        private bool[] bInput = new bool[1024];
        private bool[] bOutput = new bool[1024];
        private int[] iAD = new int[8];
        private int[] iDA = new int[8];
        //private DemoMoveMode[] moveMode = new DemoMoveMode[8];
        private int[] iTargetPos = new int[AxisMaxNum];
        private bool[] bMoveDir = new bool[AxisMaxNum];

        public double dScanTime;
        public ushort totalEthcatIn = 0;
        public ushort totalEthcatOut = 0;

        static ushort[] cardids = new ushort[8];
        static uint[] cardtypes = new uint[8];
        public ZMotionCard()
        {

        }
        public ZMotionCard(string strHareName)
            :base(strHareName)
        {
 
        }
        override public bool LoadSetting()
        {
            return true;
        }
        override public bool Init(string strName)
        {
            //ushort errorcode = 0;
            try
            {
                #region
                hardwareName = strName;
                
                cardSetting = ZMotionCardSetting.Load(strName);
                if (bInitCard == false)
                {
                    bInitCard = true;
                    zmcaux.ZAux_OpenEth(cardSetting.IpAddress, out g_handle);
                    if (g_handle == (IntPtr)0)
                    {
                        bInitOK = false;
                        return false;
                    }
                    
                }
                
                iCardNO = cardids[cardSetting.CardNo];

                for (int nAxis = 0; nAxis < cardSetting.AxisTotalCount; nAxis++)
                {
                    zmcaux.ZAux_Direct_SetAtype(g_handle, nAxis, cardSetting.AxisType[nAxis]);
                    zmcaux.ZAux_Direct_SetUnits(g_handle, nAxis, (float)cardSetting.PulsePmm[nAxis]);
                    
                    zmcaux.ZAux_Direct_SetSramp(g_handle, nAxis, 30.0f);
                    zmcaux.ZAux_Direct_Single_Addax(g_handle, nAxis, -1);           //虚拟4号轴X轴的运动叠加到实际X轴
                    zmcaux.ZAux_Direct_Single_Addax(g_handle, nAxis, -1);           //虚拟5号轴Y轴的运动叠加到实际Y轴
                }

                StartAxisID = 0;
                iTotalAxisNum = cardSetting.AxisTotalCount;

                System.Threading.Thread thread = new System.Threading.Thread(ThreadCardStatusScan);
                thread.Name = "MotionCard"+strName+"StatusScan";
                thread.IsBackground = true;
                thread.Start();

                bInitOK = true;
                #endregion
            }
            catch
            {
                //MessageBox.Show("雷塞运动控制卡初始化失败！");
            }
            return true;
        }
        int[] iHomeChannel ;
        void ThreadCardStatusScan()
        {
            
            for (int Axis_ID = StartAxisID; Axis_ID < StartAxisID+iTotalAxisNum; Axis_ID++)
            {
                
            }
            iHomeChannel = new int[iTotalAxisNum];
            for (int i = 0; i < iTotalAxisNum; i++)
            {
                zmcaux.ZAux_Direct_GetDatumIn(g_handle, i, ref iHomeChannel[i]);
            }
            GeneralLib.HiPerfTimer TIMERE = new GeneralLib.HiPerfTimer();
            while (true)
            {
                System.Threading.Thread.Sleep(1);
                try
                {
                    //lock (lockObj)
                    {
                        TIMERE.Start();
                        CardStatusScan();
                        dScanTime = TIMERE.Duration;
                    }
                }
                catch
                {

                }
            }
        }
        void CardStatusScan()
        {
            Int32 Axis_ID = 0;
           
            
            //uint sts = 0;
            for (Axis_ID = StartAxisID; Axis_ID < StartAxisID + iTotalAxisNum; Axis_ID++)
            {
                //servoOn
                int iServoStatus = 0;
                int runstate = 0;
                uint iHomeState = 0;
                int axisState = 0;
                float curCommandPos = 0;
                float curEncoderPos = 0;

                zmcaux.ZAux_Direct_GetAxisEnable(g_handle, Axis_ID, ref iServoStatus);
                zmcaux.ZAux_Direct_GetIfIdle(g_handle, Axis_ID, ref runstate);
                zmcaux.ZAux_Direct_GetDpos(g_handle, Axis_ID, ref curCommandPos);
                zmcaux.ZAux_Direct_GetMpos(g_handle, Axis_ID, ref curEncoderPos);
                zmcaux.ZAux_Direct_GetAxisStatus(g_handle, Axis_ID, ref axisState);
                zmcaux.ZAux_Direct_GetIn(g_handle, iHomeChannel[Axis_ID], ref iHomeState);
                bServoOn[Axis_ID]= iServoStatus == 0 ? false : true;
                bMoveDone[Axis_ID] = runstate == 0 ? false : true;
                iCommandPos[Axis_ID] = ((int)(curCommandPos*1000))/1000.0f;
                iCurrentPos[Axis_ID] = ((int)(curEncoderPos * 1000)) / 1000.0f;

                bCWLSenser[Axis_ID] = (((axisState >> 4) & 1) == 1)?true:false;
                bCCWLSenser[Axis_ID] = (((axisState >> 5) & 1) == 1) ? true : false;
                bAlarmSenser[Axis_ID]=(((axisState >> 22) & 1) == 1) ? true : false;
                bHomeSenser[Axis_ID] = iHomeState == 1 ? true : false;

                //bAlarmSenser[Axis_ID] = ((sts & 0x01) > 0) ? true : false;
                //bCWLSenser[Axis_ID] = ((sts & 0x02) > 0) ? true : false;
                //bCCWLSenser[Axis_ID] = ((sts & 0x04) > 0) ? true : false;
                //bHomeSenser[Axis_ID] = ((sts & 0x10) > 0) ? true : false;
                //bEstop[Axis_ID] = ((sts & 0x08) > 0) ? true : false;







            }
            ////****** Read digital output channels *****************************

            uint iGetOutValue = 0;
            zmcaux.ZAux_Direct_GetOutMulti(g_handle, 0, 16, out iGetOutValue);
            for (int i = 0; i < 16; i++)
            {
                uint iValueTemp = iGetOutValue & (uint)(0x01 << i);
                if (iValueTemp > 0)
                {
                    bOutput[i] = true;
                }
                else
                {
                    bOutput[i] = false;
                }
            }

            ////****** Read digital input channels *****************************

            int iGetInValue = 0;
            zmcaux.ZAux_Direct_GetInMulti(g_handle, 0, 16, out iGetInValue);
            for (int i = 0; i < 16; i++)
            {
                int iValueTemp = iGetInValue & (0x01 << i);
                if (iValueTemp>0)
                {
                    bInput[i] = true;
                }
                else
                {
                    bInput[i] = false;
                }
            }
            
        }
        override public bool Close()
        {
            if (bInitOK)
            {
                //lock (lockObj)
                {
                    StopALL();
                    zmcaux.ZAux_Close(g_handle);
                    g_handle = (IntPtr)0;
                }
               
            }
            return true;
        }
        override public Form ShowSettingForm()
        {
            
            FormSetting frmSetting = new FormSetting(this);
            frmSetting.TopLevel = false;
            return frmSetting;
        }
        override public bool Save()
        {
            cardSetting.Save();
            return true;
        }
        override public string GetInfo()
        {
            string strInfo = "LeiSaiMCard53032,support:\r\n" +
                "32 Axis\r\n" +
                "32 inputs,32 Outputs\r\n" +
                "8 AD Channals,8 DA Channals\r\n";
            return strInfo;
        }
        override public dynamic FunctionCaller
        {
            get
            {
                return this;
            }
        }
        public bool GetCmdPos(int iAxis,out double iPos)
        {
            iPos = (int)(iCommandPos[iAxis]*1000.0)/1000.0;
            return true;
        }
        public bool SetCmdPos(int iAxis, double iPos)
        {
            //lock (lockObj)
            {
                zmcaux.ZAux_Direct_SetDpos(g_handle, iAxis, (float)iPos);
            }

            return true;
        }
        public bool GetPos(int iAxis, out double iPos)
        {
            iPos = (int)(iCurrentPos[iAxis] * 1000.0) / 1000.0;
            return true;
        }
        public bool SetPos(int iAxis, double iPos)
        {
            lock (lockObj)
            {
                zmcaux.ZAux_Direct_SetDpos(g_handle, iAxis, (float)iPos);
                zmcaux.ZAux_Direct_SetMpos(g_handle, iAxis, (float)iPos);
            }

            return true;
        }
        public bool ZeroPos(int iAxis)
        {
            //lock (lockObj)
            {
                zmcaux.ZAux_Direct_SetDpos(g_handle, iAxis, 0);
                zmcaux.ZAux_Direct_SetMpos(g_handle, iAxis, 0);
            }

            return true;
        }
        public bool GetHome(int iAxis)
        {
            return bHomeSenser[iAxis];
        }
        public bool GetAlarm(int iAxis)
        {
            return bAlarmSenser[iAxis];
        }
        public bool GetCWL(int iAxis)
        {
            return bCWLSenser[iAxis];
        }
        public bool GetCCWL(int iAxis)
        {
          
           return bCCWLSenser[iAxis];
        }
        public bool MoveDone(int iAxis)
        {
            return bMoveDone[iAxis];
        }
        public bool StopALL()
        {
            if (bInitOK == false)
                return false;
            Int32 Axis_ID = 0;
            for (Axis_ID = 0; Axis_ID < iTotalAxisNum; Axis_ID++)
            {
                zmcaux.ZAux_Direct_Single_Cancel(g_handle, Axis_ID, 2);
            }
            return true;
        }
        public bool Stop(int iAxis)
        {


            if (bInitOK == false)
                return false;

            zmcaux.ZAux_Direct_Single_Cancel(g_handle, iAxis, 2);

            return true;
        }
        public bool EstopALL()
        {
            if (bInitOK == false)
                return false;
            Int32 Axis_ID = 0;
            for (Axis_ID = 0; Axis_ID < iTotalAxisNum; Axis_ID++)
            {
                zmcaux.ZAux_Direct_Single_Cancel(g_handle, Axis_ID, 2);
            }
            return true;
        }
        public bool EstopStop(int iAxis)
        {
            if (bInitOK == false)
                return false;
            zmcaux.ZAux_Direct_Single_Cancel(g_handle, iAxis, 2);
            return true;
        }
        public bool SetCompareOutPut(int[] iAxises, double[] iPoses,int iOutPort)
        {
           
            return true;
        }
        public bool ResetCompareOutPut(int[] iAxises)
        {
           
            return true;
        }
        public bool SetSpd(int iAxis, MotionSpeed speed)
        {
            // settingSpd[iAxis].dAcc = speed.dAcc;
            //settingSpd[iAxis].dAcc = cardSetting.Acc[iAxis];
            //settingSpd[iAxis].dDec = cardSetting.Dec[iAxis];
            settingSpd[iAxis].dAcc = speed.dAcc;
            settingSpd[iAxis].dDec = speed.dDec;
            //if (settingSpd[iAxis].dAcc < 0.05)
            //    settingSpd[iAxis].dAcc = 0.05;
            //if (settingSpd[iAxis].dDec < 0.05)
            //    settingSpd[iAxis].dDec = 0.05;
            
            settingSpd[iAxis].iMoveSpeed = speed.iMoveSpeed;
            settingSpd[iAxis].iStartSpeed = speed.iStartSpeed;
            settingSpd[iAxis].iEndSpeed = speed.iEndSpeed;
            if (iAxis == 0)
            {
                settingSpd[6].dAcc = speed.dAcc;
                settingSpd[6].dDec = speed.dDec;
                settingSpd[6].iMoveSpeed = speed.iMoveSpeed;
                settingSpd[6].iStartSpeed = speed.iStartSpeed;
                settingSpd[6].iEndSpeed = speed.iEndSpeed;
            }
            if (iAxis == 1)
            {
                settingSpd[7].dAcc = speed.dAcc;
                settingSpd[7].dDec = speed.dDec;
                settingSpd[7].iMoveSpeed = speed.iMoveSpeed;
                settingSpd[7].iStartSpeed = speed.iStartSpeed;
                settingSpd[7].iEndSpeed = speed.iEndSpeed;
            }
            //APS168.APS_set_axis_param_f(iAxis, (Int32)APS_Define.PRA_SD_DEC, speed.dDec);

            return true;
        }
        public bool Jog(int iAxis, MotionDir dir)
        {
            if (bInitOK == false)
                return false;
            //lock (lockObj)
            {
                zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis, (float)settingSpd[iAxis].iStartSpeed);
                zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis, (float)settingSpd[iAxis].iMoveSpeed);
                zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis, (float)settingSpd[iAxis].dAcc);
                zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis, (float)settingSpd[iAxis].dDec);
                int iDir = 0;
                if (dir == MotionDir.CW)
                    iDir = 1;
                else
                    iDir = -1;
                zmcaux.ZAux_Direct_Single_Vmove(g_handle, iAxis, iDir);
                bMoveDone[iAxis] = false;
            }
            return true;
        }
        public bool Home(int iAxis, MotionDir dir, HomeMode homeMode)
        {
            if (bInitOK == false)
                return false;
            //lock (lockObj)
            {
                
                zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis, (float)settingSpd[iAxis].iStartSpeed);
                zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis, (float)settingSpd[iAxis].iMoveSpeed);
                zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis, (float)settingSpd[iAxis].dAcc);
                zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis, (float)settingSpd[iAxis].dDec);
                zmcaux.ZAux_Direct_SetCreep(g_handle, iAxis, (float)settingSpd[iAxis].iStartSpeed);

                

                zmcaux.ZAux_Direct_Single_Datum(g_handle, iAxis, cardSetting.HomeMode[iAxis]);
                bMoveDone[iAxis] = false;
            }
            return true;
        }
        public bool AbsMoveSigle(int iAxis, double iPos)
        {
            if (bInitOK == false)
                return false;

            //lock (lockObj)
            {
                zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis, (float)settingSpd[iAxis].iStartSpeed);
                zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis, (float)settingSpd[iAxis].iMoveSpeed);
                zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis, (float)settingSpd[iAxis].dAcc);
                zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis, (float)settingSpd[iAxis].dDec);
                zmcaux.ZAux_Direct_Single_MoveAbs(g_handle, iAxis, (float)iPos);
                bMoveDone[iAxis] = false;
            }
            return true;
        }
        public bool RelMoveSigle(int iAxis, double iPos)
        {
            if (bInitOK == false)
                return false;

            //lock (lockObj)
            {
                zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis, (float)settingSpd[iAxis].iStartSpeed);
                zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis, (float)settingSpd[iAxis].iMoveSpeed);
                zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis, (float)settingSpd[iAxis].dAcc);
                zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis, (float)settingSpd[iAxis].dDec);
                zmcaux.ZAux_Direct_Single_Move(g_handle, iAxis, (float)iPos);
                bMoveDone[iAxis] = false;
            }
            return true;
        }
        public bool ABSMove(int[] iAxis, double[] iPos)
        {
            if (bInitOK == false)
                return false;

           
            float[] poslist =new float[iAxis.Length];
            for (int i = 0; i < iAxis.Length; i++)
            {
                poslist[i] = (float)iPos[i];
            }

            zmcaux.ZAux_Direct_Base(g_handle, 4, iAxis); //选择运动轴列表

            //插补运动使用的是主轴参数，及BASE的第一个轴
            zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iStartSpeed);
            zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);
            zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dAcc);
            zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dDec);

            if (iAxis.Length == 2)
            {
                zmcaux.ZAux_Direct_MoveAbs(g_handle, 2, iAxis, poslist);
            }
            if (iAxis.Length == 3)
            {
                zmcaux.ZAux_Direct_MoveAbs(g_handle, 3, iAxis, poslist);
            }
            if (iAxis.Length == 4)
            {
                zmcaux.ZAux_Direct_MoveAbs(g_handle, 4, iAxis, poslist);
            }
            for (int i = 0; i < iAxis.Length; i++)
                bMoveDone[iAxis[i]] = false;
            return true;

        }
        public bool RelMove(int[] iAxis, double[] iPos)
        {
            if (bInitOK == false)
                return false;


            float[] poslist = new float[iAxis.Length];
            for (int i = 0; i < iAxis.Length; i++)
            {
                poslist[i] = (float)iPos[i];
            }

            zmcaux.ZAux_Direct_Base(g_handle, 4, iAxis); //选择运动轴列表

            //插补运动使用的是主轴参数，及BASE的第一个轴
            zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iStartSpeed);
            zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);
            zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dAcc);
            zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dDec);

            if (iAxis.Length == 2)
            {
                zmcaux.ZAux_Direct_Move(g_handle, 2, iAxis, poslist);
            }
            if (iAxis.Length == 3)
            {
                zmcaux.ZAux_Direct_Move(g_handle, 3, iAxis, poslist);
            }
            if (iAxis.Length == 4)
            {
                zmcaux.ZAux_Direct_Move(g_handle, 4, iAxis, poslist);
            }
            for (int i = 0; i < iAxis.Length; i++)
                bMoveDone[iAxis[i]] = false;
            return true;
        }
        public bool ABSGo(int[] iAxis, double[] iPos)
        {
            if (bInitOK == false)
                return false;

            //lock (lockObj)
            {
                //Int32 ret = 0;

                for (int i = 0; i < iAxis.Length; i++)
                {
                    zmcaux.ZAux_Direct_SetLspeed(g_handle, i, (float)settingSpd[i].iStartSpeed);
                    zmcaux.ZAux_Direct_SetSpeed(g_handle, i, (float)settingSpd[i].iMoveSpeed);
                    zmcaux.ZAux_Direct_SetAccel(g_handle, i, (float)settingSpd[i].dAcc);
                    zmcaux.ZAux_Direct_SetDecel(g_handle, i, (float)settingSpd[i].dDec);
                    zmcaux.ZAux_Direct_Single_MoveAbs(g_handle, i, (float)iPos[i]);
                }
                for (int i = 0; i < iAxis.Length; i++)
                    bMoveDone[iAxis[i]] = false;
            }
            return true;
        }
        public bool RelGo(int[] iAxis, double[] iPos)
        {
            if (bInitOK == false)
                return false;

            //lock (lockObj)
            {
                //Int32 ret = 0;

                for (int i = 0; i < iAxis.Length; i++)
                {
                    zmcaux.ZAux_Direct_SetLspeed(g_handle, i, (float)settingSpd[i].iStartSpeed);
                    zmcaux.ZAux_Direct_SetSpeed(g_handle, i, (float)settingSpd[i].iMoveSpeed);
                    zmcaux.ZAux_Direct_SetAccel(g_handle, i, (float)settingSpd[i].dAcc);
                    zmcaux.ZAux_Direct_SetDecel(g_handle, i, (float)settingSpd[i].dDec);
                    zmcaux.ZAux_Direct_Single_Move(g_handle, i, (float)iPos[i]);
                }
                for (int i = 0; i < iAxis.Length; i++)
                    bMoveDone[iAxis[i]] = false;
            }
            return true;
        }
        public bool ABSJump(int[] iAxis, double[] iPos)
        {
            if (bInitOK == false)
                return false;
            

            float[] Poses1 = new float[iAxis.Length];
            float[] Poses2 = new float[iAxis.Length];
            float[] Poses3 = new float[iAxis.Length];
            bool bNeedJump = true;
            if (iAxis.Length == 2)
            {
                Poses1[0] = iCommandPos[iAxis[0]];
                Poses1[1] = (float)iPos[2];
                Poses2[0] = (float)iPos[0];
                Poses2[1] = (float)iPos[2];
                Poses3[0] = (float)iPos[0];
                Poses3[1] = (float)iPos[1];
                if (Math.Abs(Poses3[0] - iCommandPos[iAxis[0]]) < 200)
                {
                    bNeedJump = false;
                }
            }
            if (iAxis.Length == 3)
            {

                Poses1[0] = iCommandPos[iAxis[0]];
                Poses1[1] = iCommandPos[iAxis[1]];
                Poses1[2] = (float)iPos[3];
                Poses2[0] = (float)iPos[0];
                Poses2[1] = (float)iPos[1];
                Poses2[2] = (float)iPos[3];
                Poses3[0] = (float)iPos[0];
                Poses3[1] = (float)iPos[1];
                Poses3[2] = (float)iPos[2];
                if (Math.Abs(Poses3[0] - iCommandPos[iAxis[0]]) < 200 &&
               Math.Abs(Poses3[1] - iCommandPos[iAxis[1]]) < 200)
                {
                    bNeedJump = false;
                }
            }
            if (iAxis.Length == 4)
            {

                Poses1[0] = iCommandPos[iAxis[0]];
                Poses1[1] = iCommandPos[iAxis[1]];
                Poses1[2] = (float)iPos[4];
                Poses1[3] = iCommandPos[iAxis[3]];
                Poses2[0] = (float)iPos[0];
                Poses2[1] = (float)iPos[1];
                Poses2[2] = (float)iPos[4];
                Poses2[3] = (float)iPos[3];
                Poses3[0] = (float)iPos[0];
                Poses3[1] = (float)iPos[1];
                Poses3[2] = (float)iPos[2];
                Poses3[3] = (float)iPos[3];
                if (Math.Abs(Poses3[0] - iCommandPos[iAxis[0]]) < 0.2 &&
                Math.Abs(Poses3[1] - iCommandPos[iAxis[1]]) < 0.2 &&
                Math.Abs(Poses3[3] - iCommandPos[iAxis[3]]) < 0.2)
                {
                    bNeedJump = false;
                }
            }

            int iresult = 0;
           

            zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iStartSpeed);
            zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);
            zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dAcc);
            zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dDec);

            zmcaux.ZAux_Direct_SetMerge(g_handle, iAxis[0], 1); //打开连续插补
            
            zmcaux.ZAux_Direct_SetCornerMode(g_handle, iAxis[0], 12);//拐角模式
            //zmcaux.ZAux_Direct_SetDecelAngle(g_handle, iAxis[0], (float)(Poses1[3] * 3.14 / 180));//开始减速角度，转换为弧度
            //zmcaux.ZAux_Direct_SetStopAngle(g_handle, iAxis[0], (float)(Poses2[3] * 3.14 / 180)); //停止减速角度，转换为弧度
            zmcaux.ZAux_Direct_SetFullSpRadius(g_handle, iAxis[0], 10.0f); //小圆半径
            zmcaux.ZAux_Direct_SetZsmooth(g_handle, iAxis[0], 5.0f); //倒角

            zmcaux.ZAux_Direct_SetMovemark(g_handle, iAxis[0], 0);	//设置MARK = 0 ，来通过读取CURMARK实现判断当前执行到那里

            zmcaux.ZAux_Direct_Base(g_handle, iAxis.Length, iAxis); //base轴

            if (bNeedJump)
            {
                zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses1);
                while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
                {
                    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                    iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses1);
                }

                zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses2);
                while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
                {
                    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                    iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses2);
                }

                zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses3);
                while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
                {
                    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                    iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses3);
                }

            }
            else
            {
                zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, 4, iAxis, Poses3);
                while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
                {
                    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                    iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, Poses3);
                }
            }

            for (int i = 0; i < iAxis.Length; i++)
                bMoveDone[iAxis[i]] = false;
            return true;
        }
        public bool RelJump(int[] iAxis, double[] iPos)
        {
            return true;
        }
       
        public bool ABSArc(int[] iAxis, double[] iStart, double[] iMid, double[] iEnd, bool bCW)
        {

            if (bInitOK == false)
                return false;
           

            float[] poslist = new float[iAxis.Length];
            float[] midlist = new float[iAxis.Length];
            for (int i = 0; i < iAxis.Length; i++)
            {
                midlist[i] = (float)iMid[i]- (float)iStart[i];
                poslist[i] = (float)iEnd[i] - (float)iStart[i];
            }

            zmcaux.ZAux_Direct_Base(g_handle, 4, iAxis); //选择运动轴列表

            //插补运动使用的是主轴参数，及BASE的第一个轴
            zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iStartSpeed);
            zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);
            zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dAcc);
            zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dDec);

            if (iAxis.Length == 2)
            {
                zmcaux.ZAux_Direct_MoveCirc2Abs(g_handle, 2, iAxis, midlist[0], midlist[1], poslist[0], poslist[1]);
            }
            if (iAxis.Length == 3)
            {
                zmcaux.ZAux_Direct_MSpherical(g_handle, 3, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 0, 0, 0);
            }
            if (iAxis.Length == 4)
            {
                zmcaux.ZAux_Direct_MSpherical(g_handle, 4, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 0, poslist[3], 0);
            }
            for (int i = 0; i < iAxis.Length; i++)
                bMoveDone[iAxis[i]] = false;
            return true;
           
        }
        public bool ABSArc(int[] iAxis, double[] iStart, double[] Center, double dAngel)
        {
            if (bInitOK == false)
                return false;


            float[] poslist = new float[iAxis.Length];
            float[] midlist = new float[iAxis.Length];
            for (int i = 0; i < iAxis.Length; i++)
            {
                midlist[i] = (float)Center[i] - (float)iStart[i];
                poslist[i] = (float)iStart[i];
            }

            zmcaux.ZAux_Direct_Base(g_handle, 4, iAxis); //选择运动轴列表

            //插补运动使用的是主轴参数，及BASE的第一个轴
            zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iStartSpeed);
            zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);
            zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dAcc);
            zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dDec);

            if (iAxis.Length == 2)
            {
                zmcaux.ZAux_Direct_MSpherical(g_handle,2, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 3, 0, 0);
            }
            if (iAxis.Length == 3)
            {
                zmcaux.ZAux_Direct_MSpherical(g_handle, 3, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 3, 0, 0);
            }
            if (iAxis.Length == 4)
            {
                zmcaux.ZAux_Direct_MSpherical(g_handle, 4, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 3, poslist[3], 0);
            }
            for (int i = 0; i < iAxis.Length; i++)
                bMoveDone[iAxis[i]] = false;
            return true;
            
        }
        
        public bool InitCure(int group)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            if (bSyncMode)
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
                Axis_ID_Array[0] = 6;
                Axis_ID_Array[1] = 7;
            }
            else
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
            }
            BuildCor(Axis_ID_Array);
            return true;
        }
        public bool SetCureSpd(int group, MotionSpeed speed)
        {
            cureSpd[group] = speed;
            return true;
        }
        public bool PushLineToCure(int group, double[] iPos)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            if (bSyncMode)
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
                Axis_ID_Array[0] = 6;
                Axis_ID_Array[1] = 7;
            }
            else
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
            }
            InsertLine(Axis_ID_Array, iPos);
            return true;

        }
        public bool PushExtDo(int group, int iChannal,int iOnOff)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            if (bSyncMode)
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
                Axis_ID_Array[0] = 6;
                Axis_ID_Array[1] = 7;
            }
            else
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
            }
            //lock (lockObj)
            {
                if (iOnOff == 1)
                {
                    zmcaux.ZAux_Direct_MoveOp(g_handle, Axis_ID_Array[0], iChannal, 0);


                }
                else
                {
                    zmcaux.ZAux_Direct_MoveOp(g_handle, Axis_ID_Array[0], iChannal, 1);
                }

            }
            return true;

        }
        public bool PushArcToCure(int group, double[] iStart, double[] iMid, double[] iEnd, bool bCW,double dSweeAngle)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            if (bSyncMode)
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
                Axis_ID_Array[0] = 6;
                Axis_ID_Array[1] = 7;
            }
            else
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
            }
            InsertArc(Axis_ID_Array, iStart, iMid, iEnd, bCW, dSweeAngle);
            return true;
        }
        public bool PushArcToCure(int group, double[] iStart, double[] Center, double dAngel)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            if (bSyncMode)
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
                Axis_ID_Array[0] = 6;
                Axis_ID_Array[1] = 7;
            }
            else
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
            }
            InsertArc(Axis_ID_Array, iStart, Center, dAngel);
            return true;
        }
        public bool StartCure(int group)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
            {
                Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
            }
            StartCure(Axis_ID_Array);
            for (int i = 0; i < Axis_ID_Array.Length; i++)
            {
                bMoveDone[Axis_ID_Array[i]] = false;
            }
            return true;
        }
        public bool GetCureStatus(int group, out bool bMoveDone, out int iItemActioned)
        {
            bMoveDone = true;
            iItemActioned = 0;
            return true;
        }
        public bool GetInput(int iChanal)
        {
            return bInput[iChanal];
        }
        public bool SetOutBit(int iBit, bool bOn)
        {
            if (bInitOK == false)
                return false;
            uint iValue = bOn ? (uint)1 : (uint)0;
            zmcaux.ZAux_Direct_SetOp(g_handle, iBit, iValue);
            return true;
        }
        public bool GetOutBit(int iBit)
        {
            return bOutput[iBit];
        }
        public bool GetADValue(int iChannal, out int intValue)
        {
            intValue = iAD[iChannal];
            return true;
        }
        public bool GetDAValue(int iChannal, out int intValue)
        {
            intValue = iDA[iChannal];
            return true;
        }
        public bool SetDAValue(int iChannal, int intValue) 
        {
            iDA[iChannal] = intValue;
            return true;
        }
        private bool GetCircle(double dX1, double dY1, double dX2, double dY2, double dX3, double dY3,
            out double dCenterX, out double dCenterY, out double dR)
        {
            dCenterX = 0.0;
            dCenterY = 0.0;
            dR = 0.0;
            double dA1, dB1, dA2, dB2;
            double dCrossX = 0.0;
            double dCrossY = 0.0;
            //Line1
            bool bLine1Ok = GetLine(dX1, dY1, dX2, dY2, out dA1, out dB1);

            //Line1
            bool bLine2Ok = GetLine(dX2, dY2, dX3, dY3, out dA2, out dB2);
            //Cross

            if (bLine1Ok && bLine2Ok)
            {
                LineCross(dA1, dB1, dA2, dB2, out dCrossX, out dCrossY);
            }
            else
            {
                if (bLine1Ok == false && bLine2Ok == false)
                {
                    return false;
                }
                if (bLine1Ok == false)
                {
                    dCrossX = dB1;
                    dCenterY = dCrossX * dA2 + dB2;
                }
                if (bLine2Ok == false)
                {
                    dCrossX = dB2;
                    dCenterY = dCrossX * dA1 + dB1;
                }
            }
            dCenterX = dCrossX;
            dCenterY = dCrossY;
            dR = Math.Sqrt((dX1 - dCrossX) * (dX1 - dCrossX) + (dY1 - dCrossY) * (dY1 - dCrossY));
            return true;
        }
        private bool GetLine(double dX1, double dY1, double dX2, double dY2, out double dA, out double dB)
        {
            dA = 0.0;
            dB = 0.0;
            double dTempA, dTempB;
            double dX, dY;
            if (Math.Abs(dX1 - dX2) < 0.00000001)
            {
                dA = 0.0;
                dB = (dY1 + dY2) / 2.0;
                return true;
            }
            if (Math.Abs(dY1 - dY2) < 0.00000001)
            {
                dA = 0.0;
                dB = (dX1 + dX2) / 2.0;
                return false;
            }
            dTempA = (dY2 - dY1) / (dX2 - dX1);
            dTempB = dY1 - dA * dX1;
            //计算直线中点
            dX = (dX1 + dX2) / 2.0;
            dY = (dY1 + dY2) / 2.0;
            //中垂线
            dA = -1 / dTempA;
            dB = dY - dA * dX;
            return true;

        }
        private void LineCross(double dA1, double dB1, double dA2, double dB2, out double dCrossX, out double dCrossY)
        {
            dCrossX = 0.0;
            dCrossY = 0.0;
            dCrossX = (dB1 - dB2) / (dA2 - dA1);
            dCrossY = dA1 * dCrossX + dB1;
        }

        public bool GetServoOn(int iAxis)
        {
            return bServoOn[iAxis];
        }

        public bool SetServoOn(int iAxis, bool bOn)
        {
            //lock(lockObj)
            {
                if (bOn)
                {
                    //LTDMC.nmc_set_axis_enable((ushort)iCardNO, (ushort)iAxis);// 使能对应轴
                }
                else
                {
                    //LTDMC.nmc_set_axis_disable((ushort)iCardNO, (ushort)iAxis);// off使能对应轴
                }
            }
            return true;    
        }

        public bool RsAlarm(int iAxis)
        {
            //lock (lockObj)
            {
                //LTDMC.nmc_clear_axis_errcode((ushort)iCardNO, (ushort)iAxis);
            }
            return true;
        }

        public bool SetCatchPosInput(int iChanel, int inputChanel, int iAixsNo, bool bRaisedEdge)
        {
          
           return true;
        }

        public bool ResetCatchPosInput(int iChanel)
        {
            return true;
        }

        public bool GetCatchPos(int iChanel, out double iAxisPos)
        {
            iAxisPos = 0;
            return true;
        }

        public bool PushDelay(int group, int iDelayTimeMs)
        {
            Int32[] Axis_ID_Array = new Int32[cardSetting.GroupInfoes[group].GroupAxisNum];
            if (bSyncMode)
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
                Axis_ID_Array[0] = 6;
                Axis_ID_Array[1] = 7;
            }
            else
            {
                for (int i = 0; i < cardSetting.GroupInfoes[group].GroupAxisNum; i++)
                {
                    Axis_ID_Array[i] = cardSetting.GroupInfoes[group].AxisNum[i];
                }
            }
            zmcaux.ZAux_Direct_MoveDelay(g_handle, Axis_ID_Array[0], iDelayTimeMs);
            return true;
        }
        public void BuildCor(int[] iAxis)
        {
            zmcaux.ZAux_Direct_SetLspeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iStartSpeed);
            zmcaux.ZAux_Direct_SetSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);
            zmcaux.ZAux_Direct_SetAccel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dAcc);
            zmcaux.ZAux_Direct_SetDecel(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].dDec);

            zmcaux.ZAux_Direct_SetMerge(g_handle, iAxis[0], 1); //打开连续插补

            zmcaux.ZAux_Direct_SetCornerMode(g_handle, iAxis[0], 12);//拐角模式
            //zmcaux.ZAux_Direct_SetDecelAngle(g_handle, iAxis[0], (float)(Poses1[3] * 3.14 / 180));//开始减速角度，转换为弧度
            //zmcaux.ZAux_Direct_SetStopAngle(g_handle, iAxis[0], (float)(Poses2[3] * 3.14 / 180)); //停止减速角度，转换为弧度
            zmcaux.ZAux_Direct_SetFullSpRadius(g_handle, iAxis[0], 10.0f); //小圆半径
            zmcaux.ZAux_Direct_SetZsmooth(g_handle, iAxis[0], 5.0f); //倒角

            zmcaux.ZAux_Direct_SetMovemark(g_handle, iAxis[0], 0);	//设置MARK = 0 ，来通过读取CURMARK实现判断当前执行到那里

            zmcaux.ZAux_Direct_Base(g_handle, iAxis.Length, iAxis); //base轴

        }
        public void InsertLine(int[] iAxis, double[] iPos)
        {
            float[] poslist = new float[iAxis.Length];
            for (int i = 0; i < iAxis.Length; i++)
            {
                poslist[i] = (float)iPos[i];
            }
            zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            int iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, poslist);
            while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
            {
                zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                iresult = zmcaux.ZAux_Direct_MoveAbsSp(g_handle, iAxis.Length, iAxis, poslist);
            }

        }
        public void InsertArc(int[] iAxis, double[] iStart, double[] iMid, double[] iEnd, bool bCW, double dSweeAngle)
        {
            float[] poslist = new float[iAxis.Length];
            float[] midlist = new float[iAxis.Length];
            for (int i = 0; i < iAxis.Length; i++)
            {
                midlist[i] = (float)iMid[i] - (float)iStart[i];
                poslist[i] = (float)iEnd[i] - (float)iStart[i];
            }
           
            zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            int iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, iAxis.Length, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 0, (float)dSweeAngle, 0);
            while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
            {
                zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, iAxis.Length, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 0, (float)dSweeAngle, 0);
            }

        }
        public void InsertArc(int[] iAxis, double[] iStart, double[] Center, double dAngel)
        {
            //float[] poslist = new float[iAxis.Length];
            //float[] midlist = new float[iAxis.Length];
            //float[] endmove = new float[iAxis.Length];

            //for (int i = 0; i < iAxis.Length; i++)
            //{
            //    //zmcaux.ZAux_Direct_GetEndMoveBuffer(g_handle, iAxis[i], ref endmove[i]);   //计算相对位移
            //    poslist[i] = (float)iStart[i];
            //    midlist[i] = (float)Center[i] - (float)iStart[i];
            //    //midlist[i] = (float)Center[i] - (float)iStart[i];
            //    //poslist[i] = (float)iStart[i];
            //}
            //poslist[2] = 0.0f;
            //zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            //int iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, 4, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 3, (float)dAngel, 0);
            //while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
            //{
            //    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            //    iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, 4, iAxis, poslist[0], poslist[1], poslist[2], midlist[0], midlist[1], midlist[2], 3, (float)dAngel, 0);
            //}
            float[] midlist = new float[iAxis.Length];
            float[] endmove = new float[iAxis.Length];
            double dR = Math.Sqrt((iStart[1] - Center[1]) * (iStart[1] - Center[1]) + (iStart[0] - Center[0]) * (iStart[0] - Center[0]));
            double dStartAnglePi = Math.Atan2(iStart[1] - Center[1], iStart[0] - Center[0]);
            double dStartAngle = 180 * dStartAnglePi / Math.PI;
            double dStartAngle2;
            double dStartAngle3;
            if (dAngel > 0)
            {
                dStartAngle2 = dStartAngle + 120;
                dStartAngle3 = dStartAngle + 240;
            }
            else
            {
                dStartAngle2 = dStartAngle - 120;
                dStartAngle3 = dStartAngle - 240;
            }

            double dStartAngle2Pi = (dStartAngle2) * Math.PI / 180.0;
            double dStartAngle3Pi = (dStartAngle3) * Math.PI / 180.0;
            midlist[0] = (float)Center[0] + (float)(dR * Math.Cos(dStartAngle2Pi));
            midlist[1] = (float)Center[1] + (float)(dR * Math.Sin(dStartAngle2Pi));
            midlist[2] = (float)iStart[2];
            endmove[0] = (float)Center[0] + (float)(dR * Math.Cos(dStartAngle3Pi));
            endmove[1] = (float)Center[1] + (float)(dR * Math.Sin(dStartAngle3Pi));
            endmove[2] = (float)iStart[2];
            float[] endTranList = new float[iAxis.Length];
            float[] midTranList = new float[iAxis.Length];
            if (dAngel > 0)
            {
                for (int i = 0; i < iAxis.Length; i++)
                {
                    endTranList[i] = (float)endmove[i] - (float)iStart[i];
                    midTranList[i] = (float)midlist[i] - (float)iStart[i];
                }
            }
            else
            {
                for (int i = 0; i < iAxis.Length; i++)
                {
                    endTranList[i] = (float)endmove[i] - (float)iStart[i];
                    midTranList[i] = (float)midlist[i] - (float)iStart[i];
                }
            }

            zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            {
                int iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, 4, iAxis, endTranList[0], endTranList[1], endTranList[2], midTranList[0], midTranList[1], midTranList[2], 2, (float)dAngel, 0);
                while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
                {
                    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                    iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, 4, iAxis, endTranList[0], endTranList[1], endTranList[2], midTranList[0], midTranList[1], midTranList[2], 2, (float)dAngel, 0);
                }
            }
            else
            {
                int iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, iAxis.Length, iAxis, endTranList[0], endTranList[1], endTranList[2], midTranList[0], midTranList[1], midTranList[2], 2, (float)dAngel, 0);
                while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
                {
                    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
                    iresult = zmcaux.ZAux_Direct_MSphericalSp(g_handle, iAxis.Length, iAxis, endTranList[0], endTranList[1], endTranList[2], midTranList[0], midTranList[1], midTranList[2], 2, (float)dAngel, 0);
                }
            }
            
            //int iDirection = 0;
            //if (dAngel < 0)
            //    iDirection = 1;
            //zmcaux.ZAux_Direct_SetForceSpeed(g_handle, iAxis[0], (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            //int iresult = zmcaux.ZAux_Direct_MHelicalAbsSp(g_handle, 3, new int[] { iAxis[0], iAxis[1], iAxis[3] }, (float)iStart[0], (float)iStart[1], (float)Center[0], (float)Center[1], iDirection,(float)dAngel, 1);
            //while (iresult != 0)            //函数返回非0 则表示发送不成功，缓冲区可能满了，重新发送
            //{
            //    zmcaux.ZAux_Direct_SetForceSpeed(g_handle, 0, (float)settingSpd[iAxis[0]].iMoveSpeed);    //设置每段的速度
            //    iresult = zmcaux.ZAux_Direct_MHelicalAbsSp(g_handle, 3, new int[] { iAxis[0], iAxis[1], iAxis[3] }, (float)iStart[0], (float)iStart[1], (float)Center[0], (float)Center[1], iDirection, (float)dAngel, 1);
            //}

        }
        public void StartCure(int[] iAxis)
        {
            //lock (lockObj)
            {
                short crd = GroupJudge(iAxis);
                //if (LTDMC.dmc_conti_get_run_state((ushort)iCardNO, (ushort)crd) != 0)
                //{
                //    LTDMC.dmc_conti_start_list((ushort)iCardNO, (ushort)crd);
                //}
                //LTDMC.dmc_conti_close_list((ushort)iCardNO, (ushort)crd);
            }

        }
        private short GroupJudge(int[] iAxis)
        {
            int num = 0;
            for (int i = 0; i < cardSetting.GroupInfoes.Length; i++)
            {
                if (iAxis.Length == cardSetting.GroupInfoes[i].GroupAxisNum)
                {
                    bool bAxisMatch = true;
                    for(int j=0;j< cardSetting.GroupInfoes[i].GroupAxisNum;j++)
                    {
                        if(cardSetting.GroupInfoes[i].AxisNum[j]!=iAxis[j])
                        {
                            bAxisMatch = false;
                        }
                    }
                    if (bAxisMatch)
                    {
                        num = i;
                        break;
                    }
                }
            }
            
            return (short)num;
        }

        public void StartRegist()
        {
            //if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            //{
            //    int iret = zmcaux.ZAux_Direct_Regist(g_handle, 4, 1);
            //}
            //else
            //{
            //    int iret = zmcaux.ZAux_Direct_Regist(g_handle, 3, 1);
            //}
            if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            {
                int iret = zmcaux.ZAux_Direct_Regist(g_handle, 4, 4);
            }
            else
            {
                int iret = zmcaux.ZAux_Direct_Regist(g_handle, 3, 4);
            }

        }
        public bool GetRegistPos(out double dTragPos)
        {
            if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            {
                int iValue = 0;
                float fValue = 0;
                zmcaux.ZAux_Direct_GetMark(g_handle, 4, ref iValue);
                if (iValue == -1)//触发锁存
                {
                    int iret = zmcaux.ZAux_Direct_GetRegPos(g_handle, 4, ref fValue);
                    dTragPos = fValue;
                    return true;
                }
                else
                {
                    dTragPos = 0.0;
                    return false;
                }
            }
            else
            {
                int iValue = 0;
                float fValue = 0;
                zmcaux.ZAux_Direct_GetMark(g_handle, 3, ref iValue);
                if (iValue == -1)//触发锁存
                {
                    int iret = zmcaux.ZAux_Direct_GetRegPos(g_handle, 3, ref fValue);
                    dTragPos = fValue;
                    return true;
                }
                else
                {
                    dTragPos = 0.0;
                    return false;
                }
            }
        }
        bool bSyncMode = false;  
        public void StartMoveSync()
        {
            if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            {
                int iret = 0;

                int[] Axis = new int[2];
                float[] fWaitPos = new float[2];
                for (int i = 0; i < 2; i++)
                {
                    Axis[i] = i;
                    zmcaux.ZAux_Direct_GetMpos(g_handle, i, ref fWaitPos[i]); ;
                }
                SetPos(6, fWaitPos[0]);
                SetPos(7, fWaitPos[1]);
                bSyncMode = true;
                float fSyncAxisCurrentPos = 0;
                zmcaux.ZAux_Direct_GetMpos(g_handle, 4, ref fSyncAxisCurrentPos); ;
                iret = zmcaux.ZAux_Direct_MoveSync(g_handle, 0, 0, fSyncAxisCurrentPos, 4, 2, Axis, fWaitPos);      //同步启动加速段，
                                                                                                                    //ChangeToSyncMode(fWaitPos[0], fWaitPos[1]);
                iret = zmcaux.ZAux_Direct_MoveTable(g_handle, (uint)Axis[0], 0, (float)1);      //写TABLE数据到实际轴缓冲区，速度同步后修改TABLE触发虚拟轴
                iret = zmcaux.ZAux_Direct_MoveSync(g_handle, 0, 50000, fSyncAxisCurrentPos, 4, 2, Axis, fWaitPos);      //同步启动匀速速段，匀速时间

            }
            else
            {
                int iret = 0;

                int[] Axis = new int[2];
                float[] fWaitPos = new float[2];
                for (int i = 0; i < 2; i++)
                {
                    Axis[i] = i;
                    zmcaux.ZAux_Direct_GetMpos(g_handle, i, ref fWaitPos[i]); ;
                }
                SetPos(6, fWaitPos[0]);
                SetPos(7, fWaitPos[1]);
                bSyncMode = true;
                float fSyncAxisCurrentPos = 0;
                zmcaux.ZAux_Direct_GetMpos(g_handle, 3, ref fSyncAxisCurrentPos); ;
                iret = zmcaux.ZAux_Direct_MoveSync(g_handle, 0, 0, fSyncAxisCurrentPos, 3, 2, Axis, fWaitPos);      //同步启动加速段，
                                                                                                                    //ChangeToSyncMode(fWaitPos[0], fWaitPos[1]);
                iret = zmcaux.ZAux_Direct_MoveTable(g_handle, (uint)Axis[0], 0, (float)1);      //写TABLE数据到实际轴缓冲区，速度同步后修改TABLE触发虚拟轴
                iret = zmcaux.ZAux_Direct_MoveSync(g_handle, 0, 50000, fSyncAxisCurrentPos, 3, 2, Axis, fWaitPos);
            }

        }
        public void StopMoveSync(double dPosX,double dPosY)
        {
            if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            {
                bSyncMode = false;
                int[] Axis = new int[2];
                float[] fWaitPos = new float[2];
                for (int i = 0; i < 2; i++)
                {
                    Axis[i] = i;
                    zmcaux.ZAux_Direct_GetMpos(g_handle, i, ref fWaitPos[i]); ;
                }
                //fWaitPos[0] = (float)dPosX;
                //fWaitPos[1] = (float)dPosY;
                int iret;
                //iret = zmcaux.ZAux_Direct_MoveWait(g_handle, (uint)6, "TABLE", 0, 0, (float)0);
                iret = zmcaux.ZAux_Direct_MoveSync(g_handle, -2, 0, 0, 4, 2, Axis, fWaitPos);

                //iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 0, -1);           //虚拟4号轴X轴的运动叠加到实际X轴
                //iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 1, -1);           //虚拟5号轴Y轴的运动叠加到实际Y轴
                iret = zmcaux.ZAux_Direct_MoveTable(g_handle, (uint)Axis[0], 0, (float)0);
            }
            else
            {
                bSyncMode = false;
                int[] Axis = new int[2];
                float[] fWaitPos = new float[2];
                for (int i = 0; i < 2; i++)
                {
                    Axis[i] = i;
                    zmcaux.ZAux_Direct_GetMpos(g_handle, i, ref fWaitPos[i]); ;
                }
                //fWaitPos[0] = (float)dPosX;
                //fWaitPos[1] = (float)dPosY;
                int iret;
                //iret = zmcaux.ZAux_Direct_MoveWait(g_handle, (uint)6, "TABLE", 0, 0, (float)0);
                iret = zmcaux.ZAux_Direct_MoveSync(g_handle, -2, 0, 0, 3, 2, Axis, fWaitPos);

                //iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 0, -1);           //虚拟4号轴X轴的运动叠加到实际X轴
                //iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 1, -1);           //虚拟5号轴Y轴的运动叠加到实际Y轴
                iret = zmcaux.ZAux_Direct_MoveTable(g_handle, (uint)Axis[0], 0, (float)0);
            }

        }


        public void StartAdd()
        {
            int iret = 0;

            int[] Axis = new int[2];
            float[] fWaitPos = new float[2];
            for (int i = 0; i < 2; i++)
            {
                Axis[i] = i;
                zmcaux.ZAux_Direct_GetMpos(g_handle, i, ref fWaitPos[i]); ;
            }
            SetPos(6, fWaitPos[0]);
            SetPos(7, fWaitPos[1]);
            bSyncMode = true;
            iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 0, 6);           //虚拟4号轴X轴的运动叠加到实际X轴
            iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 1, 7);           //虚拟5号轴Y轴的运动叠加到实际Y轴
           

        }
        public void StopAdd()
        {
            bSyncMode = false;
            int iret;
            iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 0, -1);           //虚拟4号轴X轴的运动叠加到实际X轴
            iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 1, -1);           //虚拟5号轴Y轴的运动叠加到实际Y轴


        }

        public void ChangeToSyncMode(double dXCurrentPOS, double dYCurrentPOS)
        {
            bSyncMode = true;
            int iret = 0;
           
            iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 0, 6);           //虚拟4号轴X轴的运动叠加到实际X轴
            iret = zmcaux.ZAux_Direct_Single_Addax(g_handle, 1, 7);           //虚拟5号轴Y轴的运动叠加到实际Y轴
            //iret = zmcaux.ZAux_Direct_MoveWait(g_handle, (uint)6, "TABLE", 0, 0, (float)1);            //等待跟随实轴达到速度同步 TABLE(0) = 1  
        }
        public bool WaitCureMoveDone()
        {
            if (GeneralDefine.GlueMachineSetting.Is4AxisMachine)
            {
                int irunstate1 = 0;
                int irunstate2 = 0;
                int irunstate3 = 0;
                int irunstate4 = 0;
                while (true)
                {
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 6, ref irunstate1);
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 7, ref irunstate2);
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 2, ref irunstate3);
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 3, ref irunstate4);
                    if (irunstate1 != 0 && irunstate2 != 0 && irunstate3 != 0 && irunstate4 != 0)
                    {
                        break;
                    }
                    System.Threading.Thread.Sleep(10);

                }
                return true;
            }
            else
            {
                int irunstate1 = 0;
                int irunstate2 = 0;
                int irunstate3 = 0;

                while (true)
                {
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 6, ref irunstate1);
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 7, ref irunstate2);
                    zmcaux.ZAux_Direct_GetIfIdle(g_handle, 2, ref irunstate3);

                    if (irunstate1 != 0 && irunstate2 != 0 && irunstate3 != 0)
                    {
                        break;
                    }
                    System.Threading.Thread.Sleep(10);

                }
                return true;
            }
            
        }
    }
}
